#include "grab.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "message_center.h"
#include "general_def.h"
#include "bsp_gpio.h"

static Subscriber_t *grab_sub;
static Publisher_t *grab_pub;

static Grab_Ctrl_Cmd_s grab_cmd_recv;
static Grab_feed_info_t grab_feed;
static DJIMotorInstance *grab_pitch, *grab_forward;
static DJIMotorInstance *grab_roll;
static GPIOInstance *sucker_1, *sucker_2;

static float forward_angle, pitch_angle, roll_angle;
static float forward_angle_flag, pitch_angle_flag, roll_angle_flag;
static float start_angle_forward;
static float start_angle_pitch;


// tbd 确定限位值?
#define PITCH_OUTWARD_LIMIT 0.5
#define PITCH_INWARDE_LIMIT -0.5
#define STRECH_FRONT_LIMIT 0.5
#define STRECH_BACK_LIMIT -0.5
#define ROLL_CLOCKWISE_LIMIT 0.5
#define ROLL_COUNTER_CLOCKWISE_LIMIT -0.5

void GrabInit()
{
    Motor_Init_Config_s conf_pitch = {
        .can_init_config = {
            .can_handle = &hcan1,
            .tx_id = 1,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 7,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 15000,
                .DeadBand = 0.1,
            },
            .speed_PID = {
                .Kp = 3,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 15000,
                .Improve = PID_ErrorHandle,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = M3508,
    };
    pitch_angle = 0;
    pitch_angle_flag = 1;
    grab_pitch = DJIMotorInit(&conf_pitch);
    start_angle_pitch = grab_pitch->measure.total_angle;

    Motor_Init_Config_s conf_forward = {
        .can_init_config = {
            .can_handle = &hcan1,
            .tx_id = 2,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 7,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 15000,
                .DeadBand = 0.1,
            },
            .speed_PID = {
                .Kp = 3,
                .Ki = 0,
                .IntegralLimit = 1000,
                .Improve = PID_Integral_Limit,
                .Kd = 0,
                .MaxOut = 15000,
                .Improve = PID_ErrorHandle,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = M3508,
    };
    forward_angle = 0;
    forward_angle_flag = 1;
    grab_forward = DJIMotorInit(&conf_forward);

    start_angle_forward = grab_forward->measure.total_angle;

    Motor_Init_Config_s conf_roll = {
        .can_init_config = {
            .can_handle = &hcan1,
            .tx_id = 6,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 7,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 10000,
                .DeadBand = 0.1,
            },
            .speed_PID = {
                .Kp = 3,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 10000,
                .Improve = PID_ErrorHandle,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = M2006,
    };
    roll_angle = 0;
    roll_angle_flag = 1;
    grab_roll = DJIMotorInit(&conf_roll);

    // 吸盘继电器
    GPIO_Init_Config_s conf_sucker_1 = {
        .GPIOx = GPIOE,
        .GPIO_Pin = GPIO_PIN_13,
        .pin_state = GPIO_PIN_RESET,
    };
    sucker_1 = GPIORegister(&conf_sucker_1);
    GPIO_Init_Config_s conf_sucker_2 = {
        .GPIOx = GPIOE,
        .GPIO_Pin = GPIO_PIN_14,
        .pin_state = GPIO_PIN_RESET,
    };
    sucker_2 = GPIORegister(&conf_sucker_2);

    grab_sub = SubRegister("grab_cmd", sizeof(Grab_Ctrl_Cmd_s));
    grab_pub = PubRegister("grab_feed", sizeof(Grab_feed_info_t));
}

/* 机器人夹爪控制核心任务 */
void GrabTask()
{
    if (roll_angle_flag)
    {
        roll_angle = grab_roll->measure.total_angle;
        roll_angle_flag = 0;
    }
    if (forward_angle_flag)
    {
        forward_angle = grab_forward->measure.total_angle;
        forward_angle_flag = 0;
    }
    if (pitch_angle_flag)
    {
        pitch_angle = grab_pitch->measure.total_angle;
        pitch_angle_flag = 0;
    }

    SubGetMessage(grab_sub, &grab_cmd_recv);

    if (grab_cmd_recv.sucker_flag) // 吸盘开关
    {
        GPIOSet(sucker_1);
        GPIOSet(sucker_2);
    }
    else
    {
        GPIOReset(sucker_1);
        GPIOReset(sucker_2);
    }

    // 切换夹爪运行速度
    static float speed_coef;
    if (grab_cmd_recv.grab_mode == GRAB_FAST)
        speed_coef = 1;
    else if (grab_cmd_recv.grab_mode == GRAB_SLOW)
        speed_coef = 0.3;

    // 夹爪Roll控制
    if (grab_cmd_recv.clock_wise_flag != 0)
    {
        DJIMotorOuterLoop(grab_roll, SPEED_LOOP);
        DJIMotorSetRef(grab_roll, grab_cmd_recv.clock_wise_flag * speed_coef * 10000); // tbd 调试夹爪roll转速
        roll_angle = grab_roll->measure.total_angle;
    }
    else
    {
        DJIMotorOuterLoop(grab_roll, ANGLE_LOOP); // tbd 增加外层闭环切换
        DJIMotorSetRef(grab_roll, roll_angle);
    }

    // 夹爪pitch控制,左下右中遥感左——上下
    if (grab_cmd_recv.pitch_inward_flag != 0)
    {
        DJIMotorOuterLoop(grab_pitch, SPEED_LOOP);
        DJIMotorSetRef(grab_pitch, grab_cmd_recv.pitch_inward_flag * speed_coef * 10000); // 调试夹爪pitch转速
        pitch_angle = grab_pitch->measure.total_angle;
    }
    else
    {
        DJIMotorOuterLoop(grab_pitch, ANGLE_LOOP);
        DJIMotorSetRef(grab_pitch, pitch_angle); // 调试夹爪pitch转速
    }

    // 夹爪前伸,左下右中遥感右——左右
    if (grab_cmd_recv.forward_flag != 0)
    {
        DJIMotorOuterLoop(grab_forward, SPEED_LOOP);
        DJIMotorSetRef(grab_forward, grab_cmd_recv.forward_flag * speed_coef * 10000); // 调试夹爪前伸转速
        forward_angle = grab_forward->measure.total_angle;
    }
    else
    {
        DJIMotorOuterLoop(grab_forward, ANGLE_LOOP);
        DJIMotorSetRef(grab_forward, forward_angle);
    }

    // 处理紧急情况
    if (grab_cmd_recv.grab_mode == GRAB_OFF)
    {
        DJIMotorStop(grab_pitch);
        DJIMotorStop(grab_forward);
        DJIMotorStop(grab_roll);
        GPIOReset(sucker_1);
        GPIOReset(sucker_2);
    }
    else
    {
        DJIMotorEnable(grab_pitch);
        DJIMotorEnable(grab_forward);
        DJIMotorEnable(grab_roll);
    }
    grab_feed.forward_start_angle = start_angle_forward;
    grab_feed.forward_now_angle = grab_forward->measure.total_angle;
    grab_feed.forward_start_angle = start_angle_pitch;
    grab_feed.pitch_now_angle = grab_pitch->measure.total_angle;
    PubPushMessage(grab_pub, (void *)&grab_feed);
}
